package com.dmatek.uwb.local.algorithm;
import java.util.ArrayList;
import java.util.List;

import com.dmatek.uwb.packet.bean.IterateDataFormat;
import com.dmatek.uwb.packet.bean.Matrix;
/***
 * 
 * @author zf
 * @deprecated
 */
public enum LocalAlgorithm 
{
	//单例模式
	INSTANCE;
	private final int CycleNum = 100;
	public IterateDataFormat dimenIndoorPosAlgorithm(List<IterateDataFormat> iterates)
	{
		double x0 = 0,y0 = 0,z0 = 0;
		try {
			double[][] a = new double[iterates.size()][4];
	        for (int i = 0; i < iterates.size(); i++) {
	            a[i][0] = 1;
	            a[i][1] = -2 * iterates.get(i).x;
	            a[i][2] = -2 * iterates.get(i).y;
	            a[i][3] = -2 * iterates.get(i).z;
	        }
	        Matrix A = new Matrix(a);
	        double[][] b = new double[iterates.size()][1];
	        for (int i = 0; i < iterates.size(); i++)
	        {
	            b[i][0] = iterates.get(i).r * iterates.get(i).r - iterates.get(i).x * iterates.get(i).x - iterates.get(i).y * iterates.get(i).y - iterates.get(i).z * iterates.get(i).z;
	        }
	        
	        Matrix B = new Matrix(b);
	        Matrix At = A.Transpose();
	        Matrix AtA = At.PMulti(A);
	        Matrix A1 = AtA.Inverse();
	        Matrix X = A1.PMulti(At).PMulti(B);
	        
	        double[][] g = new double[4][3];
	        g[0][0] = 1; g[0][1] = 0; g[0][2] = 0;
	        g[1][0] = 0; g[1][1] = 1; g[1][2] = 0;
	        g[2][0] = 0; g[2][1] = 0; g[2][2] = 1;
	        g[3][0] = 1; g[3][1] = 1; g[3][2] = 1;
	        Matrix G = new Matrix(g);
	        double[][] h = new double[4][1];
	        h[0][0] = X.getM()[1][0] * X.getM()[1][0];
	        h[1][0] = X.getM()[2][0] * X.getM()[2][0];
	        h[2][0] = X.getM()[3][0] * X.getM()[3][0];
	        h[3][0] = X.getM()[0][0];
	        Matrix H = new Matrix(h);
	        Matrix    GT = G.Transpose();
	        Matrix    GTG = GT.PMulti(G);
	        Matrix    G1 = GTG.Inverse();
	        Matrix    X1 = G1.PMulti(GT).PMulti(H);
	        if (X1.getM()[0][0] < 0 || X1.getM()[1][0] < 0 || X1.getM()[2][0] < 0)
	        {
	            x0 = X.getM()[1][0];
	            y0 = X.getM()[2][0];
	            z0 = X.getM()[3][0];
	        }
	        else
	        {
	            if (X.getM()[1][0] >= 0) x0 = Math.sqrt(X1.getM()[0][0]);
	            else x0 = -Math.sqrt(X1.getM()[0][0]);
	
	            if (X.getM()[2][0] >= 0) y0 = Math.sqrt(X1.getM()[1][0]);
	            else y0 = -Math.sqrt(X1.getM()[1][0]);
	
	            if (X.getM()[3][0] >= 0) z0 = Math.sqrt(X1.getM()[2][0]);
	            else z0 = -Math.sqrt(X1.getM()[2][0]);
	        }
		}catch(Exception ex)
		{
            double minR = Double.MAX_VALUE;
            for (int i = 0; i < iterates.size(); i++)
            {
                if (iterates.get(i).r <= minR)
                {
                    minR = iterates.get(i).r;
                    x0 = iterates.get(i).x;
                    y0 = iterates.get(i).y;
                    z0 = iterates.get(i).z;
                }
            }
		}
		IterateDataFormat initData = new IterateDataFormat(x0, y0, z0, 0);
        IterateDataFormat optimumData = null;
        //使用泰勒迭代
        try {
			optimumData = taylorIterator(initData, iterates, 5);
		} catch (Exception e) {
			e.printStackTrace();
		}
        if (optimumData.getX() == Double.MAX_VALUE && optimumData.getY() == Double.MAX_VALUE 
         && optimumData.getR() == Double.MAX_VALUE && optimumData.getZ() == Double.MAX_VALUE) {
        	//选取合适的步数
            double step = 0;
            for (int i = 0; i < iterates.size(); i++)
            {
                double dis = Math.sqrt((x0 - iterates.get(i).getX()) * (x0 - iterates.get(i).getX()) + (y0 - iterates.get(i).getY()) * (y0 - iterates.get(i).getY()) + (z0 - iterates.get(i).getZ()) * (z0 - iterates.get(i).getZ())) + iterates.get(i).getR();
                if (dis >= step)
                {
                    step = dis;
                }
            }
            step = (int)step;
            optimumData = initData;
            //防止陷入死循环
            int count = CycleNum;
            for ( ; ; )
            {
            	count --;
                optimumData = ManualIterate(optimumData, iterates, step);
                step = step / 4;
                step = (int)step;
                if (step <= 1)
                {
                    break;
                }
                if(count <= 0)
                {//防止死循环
                	break;
                }
            }
        }
        x0 = optimumData.x;
        y0 = optimumData.y;
        z0 = optimumData.z;
        return new IterateDataFormat(x0, y0, z0, 0);
	}
    private IterateDataFormat ManualIterate(IterateDataFormat initData, List<IterateDataFormat> iterateData, double step)
    {
        IterateDataFormat lastData = new IterateDataFormat(initData.x, initData.y, initData.z, initData.r);
        double squareSumBase = 0;
        double squareSumTemp = 0;
        IterateDataFormat dataTemp = new IterateDataFormat(0, 0, 0, 0);
        int num = 0,count = 0;
        while (true)
        {
        	num ++;
            squareSumBase = SumOfSquare(lastData, iterateData);
            dataTemp.x = lastData.x;
            dataTemp.y = lastData.y;
            dataTemp.z = lastData.z;
            //先对x坐标进行迭代
            dataTemp.x += step;
            //X坐标加，可以减少平方和
            squareSumTemp = SumOfSquare(dataTemp, iterateData);
            if (squareSumTemp < squareSumBase) {
            	count = CycleNum;
                while (count > 0) {
                    squareSumBase = squareSumTemp;
                    dataTemp.x += step;
                    squareSumTemp = SumOfSquare(dataTemp, iterateData);
                    count --;
                    if (squareSumTemp > squareSumBase)
                        break;
                }
                dataTemp.x -= step;
            } //X坐标减，可以减少平方和
            else {
                dataTemp.x = lastData.x;
                squareSumTemp = squareSumBase;
                count = CycleNum;
                while (count > 0) {
                    squareSumBase = squareSumTemp;
                    dataTemp.x -= step;
                    squareSumTemp = SumOfSquare(dataTemp, iterateData);
                    count --;
                    if (squareSumTemp > squareSumBase)
                        break;
                }
                dataTemp.x += step;
            }
            //对Y坐标进行迭代
            dataTemp.y += step;
            //Y坐标加，可以减少平方和
            squareSumTemp = SumOfSquare(dataTemp, iterateData);
            if (squareSumTemp < squareSumBase) {
            	count = CycleNum;
                while (count > 0) {
                    squareSumBase = squareSumTemp;
                    dataTemp.y += step;
                    squareSumTemp = SumOfSquare(dataTemp, iterateData);
                    count --;
                    if (squareSumTemp > squareSumBase)
                        break;
                }
                dataTemp.y -= step;
            }
            //Y坐标减，可以减少平方和
            else
            {
                dataTemp.y = lastData.y;
                squareSumTemp = squareSumBase;
                count = CycleNum;
                while (count > 0) {
                    squareSumBase = squareSumTemp;
                    dataTemp.y -= step;
                    squareSumTemp = SumOfSquare(dataTemp, iterateData);
                    count --;
                    if (squareSumTemp > squareSumBase)
                        break;
                }
                dataTemp.y += step;
            }
            //对Z坐标进行迭代
            dataTemp.z += step;
            //Z坐标加，可以减少平方和
            squareSumTemp = SumOfSquare(dataTemp, iterateData);
            if (squareSumTemp < squareSumBase) {
            	count = CycleNum;
                while (count > 0) {
                    squareSumBase = squareSumTemp;
                    dataTemp.z += step;
                    squareSumTemp = SumOfSquare(dataTemp, iterateData);
                    count --;
                    if (squareSumTemp > squareSumBase)
                        break;
                }
                dataTemp.z -= step;
            } //Z坐标减，可以减少平方和
            else {
                dataTemp.z = lastData.z;
                squareSumTemp = squareSumBase;
                count = CycleNum;
                while (count > 0) {
                    squareSumBase = squareSumTemp;
                    dataTemp.z -= step;
                    squareSumTemp = SumOfSquare(dataTemp, iterateData);
                    count --;
                    if (squareSumTemp > squareSumBase)
                        break;
                }
                dataTemp.z += step;
            }
            //迭代后的最优解与上次的一致，迭代结束
            if (dataTemp.x == lastData.x && dataTemp.y == lastData.y && dataTemp.z == lastData.z) {
                return lastData;
            }
        	if(num > 20) {
        		return lastData;
        	}
            lastData.x = dataTemp.x;
            lastData.y = dataTemp.y;
            lastData.z = dataTemp.z;
        }
    }
    private double SumOfSquare(IterateDataFormat initData, List<IterateDataFormat> iterateData)
    {
        double squareSum = 0;
        for (int i = 0; i < iterateData.size(); i++)
        {
            squareSum += (Math.abs(Math.sqrt((initData.x - iterateData.get(i).x) * (initData.x - iterateData.get(i).x) + (initData.y - iterateData.get(i).y) * (initData.y - iterateData.get(i).y) + (initData.z - iterateData.get(i).z) * (initData.z - iterateData.get(i).z)) - iterateData.get(i).r) *
                          Math.abs(Math.sqrt((initData.x - iterateData.get(i).x) * (initData.x - iterateData.get(i).x) + (initData.y - iterateData.get(i).y) * (initData.y - iterateData.get(i).y) + (initData.z - iterateData.get(i).z) * (initData.z - iterateData.get(i).z)) - iterateData.get(i).r));
        }
        return squareSum;
    }
	private IterateDataFormat taylorIterator(IterateDataFormat init,List<IterateDataFormat> iterates,double threshold) throws Exception
	{
		IterateDataFormat resformat = new IterateDataFormat(init.getX(),init.getY(),init.getZ(),init.getR());
		ArrayList<Double> rnoList = new ArrayList<Double>();
		int count = 0;
		double lastDelta = Double.MAX_VALUE;
		try
		{
			while(true)
			{
				count++;
				rnoList.clear();
				for(int i = 0;i < iterates.size();i ++)
				{
					double rno = Math.pow((iterates.get(i).x - resformat.x) * (iterates.get(i).x - resformat.x)  + (iterates.get(i).y - resformat.y) * (iterates.get(i).y - resformat.y) + (iterates.get(i).z - resformat.z) * (iterates.get(i).z - resformat.z), 0.5);
					rnoList.add(rno);
				}
				double[][] ah = new double[iterates.size()][1];
				for(int i = 0;i < iterates.size();i ++)
				{
					ah[i][0] = iterates.get(i).getR() - rnoList.get(i);
				}
				Matrix Mh = new Matrix(ah);
		        double[][] ag = new double[iterates.size()][3];
		        for (int i = 0; i < iterates.size(); i++)
		        {
		        	ag[i][0] = (init.x - iterates.get(i).x) / rnoList.get(i);
		        	ag[i][1] = (init.y - iterates.get(i).y) / rnoList.get(i);
		        	ag[i][2] = (init.z - iterates.get(i).z) / rnoList.get(i);
		        }
				Matrix Mg = new Matrix(ag);
		        Matrix MgT = Mg.Transpose();
				Matrix MgTMg = MgT.PMulti(Mg);
		        Matrix MgTMg1 = MgTMg.Inverse();
		        Matrix Mdelta = MgTMg1.PMulti(MgT).PMulti(Mh);
		        double[][] M = Mdelta.getM();
		        if(lastDelta < Math.abs(M[0][0]) + Math.abs(M[1][0]) + Math.abs(M[2][0]))
		        {
		        	resformat.setX(Double.MAX_VALUE);
		        	resformat.setY(Double.MAX_VALUE);
		        	resformat.setZ(Double.MAX_VALUE);
		        	resformat.setR(Double.MAX_VALUE);
		        	return resformat;
		        }
		        lastDelta = Math.abs(M[0][0]) + Math.abs(M[1][0]) + Math.abs(M[2][0]);
		        resformat.setX(resformat.getX() + M[0][0]);
		        resformat.setY(resformat.getY() + M[1][0]);
		        resformat.setY(resformat.getZ() + M[2][0]);
		        if(count > 20)
		        {
		        	resformat.setX(Double.MAX_VALUE);
		        	resformat.setY(Double.MAX_VALUE);
		        	resformat.setZ(Double.MAX_VALUE);
		        	resformat.setR(Double.MAX_VALUE);
		        	return resformat;
		        }
			}
		}catch(Exception ex)
		{
        	resformat.setX(Double.MAX_VALUE);
        	resformat.setY(Double.MAX_VALUE);
        	resformat.setZ(Double.MAX_VALUE);
        	resformat.setR(Double.MAX_VALUE);
        	return resformat;
		}
	}
}